package org.nashua.tt151;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SimpleRobot;
import org.nashua.tt151.Controller.DualAction;
import org.nashua.tt151.Controller.MSKinect;
import org.nashua.tt151.RobotMap.Jaguar.Power;

public class CrimsonChaos extends SimpleRobot {
    /* Hardware */
    public static Jaguar backLeft = new Jaguar(RobotMap.Jaguar.BACK_LEFT);
    public static Jaguar backRight = new Jaguar(RobotMap.Jaguar.BACK_RIGHT);
    public static Jaguar frontLeft = new Jaguar(RobotMap.Jaguar.FRONT_LEFT);
    public static Jaguar frontRight = new Jaguar(RobotMap.Jaguar.FRONT_RIGHT);
    public static Jaguar pulley = new Jaguar(RobotMap.Jaguar.PULLEY);
    public static Jaguar roller = new Jaguar(RobotMap.Jaguar.ROLLER);
    public static DualAction joy = new DualAction(1);
    public static MSKinect leftArm = new MSKinect(MSKinect.LEFT_ARM);
    public static MSKinect rightArm = new MSKinect(MSKinect.RIGHT_ARM);

    /* Other Variables */
    public static double drivePower = Power.DEFAULT_POWER;

    /**
     * This method is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        reset();
        leftArm.addButtonListener(MSKinect.Button.LEFT_LEG_FWD, new Controller.ButtonListener() {
            public void onPress(boolean opt) {
                pulley.set(leftArm.getRawButton(MSKinect.Button.RIGHT_LEG_FWD) ? -1 : 1);
                /*
                 * if (leftArm.getRawButton(MSKinect.Button.RIGHT_LEG_FWD) {
                 *      pulley.set(-1);  // Reversed When Right Leg FORWARD
                 * } else {
                 *      pulley.set(1);  // Forward
                 * }
                 */
            }
            public void onRelease(boolean opt) {
                pulley.set(0);
            }
        });
        leftArm.addButtonListener(MSKinect.Button.RIGHT_LEG_BACK, new Controller.ButtonListener() {
            public void onPress(boolean opt) {
                roller.set(leftArm.getRawButton(MSKinect.Button.LEFT_LEG_BACK) ? -1 : 1);
                /*
                 * if (leftArm.getRawButton(MSKinect.Button.LEFT_LEG_BACK) {
                 *      roller.set(-1);  // Reversed When Left Leg BACK
                 * } else {
                 *      roller.set(1);  // Forward
                 * }
                 */
            }
            public void onRelease(boolean opt) {
                roller.set(0);
            }
        });
        while (isEnabled() && isAutonomous()) {
            leftArm.queryButtons();
            rightArm.queryButtons();

            drivePower = power(leftArm, MSKinect.Button.HEAD_LEFT,
                    MSKinect.Button.HEAD_RIGHT);

            double left = leftArm.getY() * drivePower;
            double right = -rightArm.getY() * drivePower;
            backLeft.set(left);
            frontLeft.set(left);
            backRight.set(right);
            frontRight.set(right);
        }
        reset();
    }

    /**
     * This method is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        reset();
        joy.addButtonListener(DualAction.Button.LEFT_BUMPER, new Controller.ButtonListener() {
            public void onPress(boolean opt) {
                pulley.set(joy.getRawButton(DualAction.Button.BACK) ? -1 : 1);
                /*
                 * if (joy.getRawButton(DualAction.Button.BACK) {
                 *      pulley.set(-1);  // Reversed When BACK is Down
                 * } else {
                 *      pulley.set(1);  // Forward
                 * }
                 */
            }
            public void onRelease(boolean opt) {
                pulley.set(0);
            }
        });
        joy.addButtonListener(DualAction.Button.RIGHT_BUMPER, new Controller.ButtonListener() {
            public void onPress(boolean opt) {
                roller.set(joy.getRawButton(DualAction.Button.SELECT) ? -1 : 1);
                /*
                 * if (joy.getRawButton(DualAction.Button.SELECT) {
                 *      roller.set(-1);  // Reversed When SELECT is Down
                 * } else {
                 *      roller.set(1);  // Forward
                 * }
                 */
            }
            public void onRelease(boolean opt) {
                roller.set(0);
            }
        });
        while (isEnabled() && isOperatorControl()) {
            joy.queryButtons();

            drivePower = power(joy, DualAction.Button.LEFT_TRIGGER,
                    DualAction.Button.RIGHT_TRIGGER);

            double left = joy.getLeftY() * drivePower;
            double right = -joy.getRightY() * drivePower;
            backLeft.set(left);
            frontLeft.set(left);
            backRight.set(right);
            frontRight.set(right);
        }
        reset();
    }

    public double power(Controller c, int button_creep, int button_max) {
        if (c.getRawButton(button_creep))
            return Power.CREEP_POWER;
        if (c.getRawButton(button_max))
            return Power.MAX_POWER;
        return Power.DEFAULT_POWER;
    }

    public void reset() {
        joy.clearAll();  // Reset DualAction to verify that it has no control
        leftArm.clearAll();  // Reset MSKinect to verify that it has no control
        rightArm.clearAll();  // Reset MSKinect to verify that it has no control
    }
}
